#include "algorithm.h"

#include <set>
#include <vector>
#include <cmath>

namespace Algorithm {
	typedef std::pair<Index, float> PointScale;

	float LoG(Bitmap<float>& src, Filter<float>* gauss, int x, int y, int max_width, int max_height) {
		int maxx = gauss->getMaxX();
		Bitmap<float> subpic(src, x - maxx, y - maxx, 2*maxx + 1, 2*maxx + 1); 
		Bitmap<float>* gp = gauss->Run(subpic);

		float top = gp->getPixel(maxx, maxx-1);
		float bottom = gp->getPixel(maxx, maxx+1);
		float left = gp->getPixel(maxx-1, maxx);
		float right = gp->getPixel(maxx+1, maxx);
		float center = gp->getPixel(maxx, maxx);
					
		float lXX = (top -2.0f*center + bottom) / 255.0;
		float lYY = (left -2.0f*center + right) / 255.0;

		delete gp;

		return abs(lXX + lYY);
	}
	
	float calcMeasure(Bitmap<float>& src, Filter<float>* diffGauss, Filter<float3>* intGauss, int2 pixel, float lambda, float od)
	{
		// calculate m
		int maxx = diffGauss->getMaxX() + 1 + intGauss->getMaxX();
		int maxy = diffGauss->getMaxY() + 1 + intGauss->getMaxY();
		int minx = diffGauss->getMinX() + 1 + intGauss->getMinX();
		int miny = diffGauss->getMinY() + 1 + intGauss->getMinY();
		Bitmap<float> subpic(src, pixel.x + minx, (int)pixel.y + miny, -minx + maxx + 1, -miny + maxy + 1);
						
		Bitmap<float>* diffeGauss_pic = diffGauss->Run(subpic);
		Bitmap<float3>* DIFF_O_diffGaussed_pic = new Bitmap<float3>(diffeGauss_pic->getWidth()-2, diffeGauss_pic->getHeight()-2);
		for(unsigned i = 1; i<diffeGauss_pic->getWidth()-1; ++i)
		{
			for(unsigned j = 1; j<diffeGauss_pic->getHeight()-1; ++j)
			{
				float3& pixel = DIFF_O_diffGaussed_pic->getPixel(i, j);
				float dX = diffeGauss_pic->getPixel(i+1, j) - diffeGauss_pic->getPixel(i-1, j);
				float dY = diffeGauss_pic->getPixel(i, j+1) - diffeGauss_pic->getPixel(i, j-1);
				pixel.x = dX*dX;
				pixel.y = dX*dY;
				pixel.z = dY*dY;
			}
		}
		
		Bitmap<float3>* intGaussed_O_DIFF_O_diffGaussed_pic = intGauss->Run(*DIFF_O_diffGaussed_pic);

						//calculat labda1,2
		float3 pix = intGaussed_O_DIFF_O_diffGaussed_pic->getPixel(pixel.x, pixel.y);
		float tr = pix.x + pix.z;
		float det = pix.x*pix.z - pix.y*pix.y;
		float sqrDiffSigma = od*od;
		float qvadDiffSigma = sqrDiffSigma*sqrDiffSigma;
		float me = /*abs*/(det)*qvadDiffSigma - lambda*tr*tr*sqrDiffSigma;
		
		delete diffeGauss_pic;
		delete DIFF_O_diffGaussed_pic;
		delete intGaussed_O_DIFF_O_diffGaussed_pic;
		return me;			
	}

	float2x2 calcMeasureMex(int& x, int& y, Bitmap<float>& src, Filter<float>* diffGauss, Filter<float3>* intGauss, int2 pixel, float lambda, float od)
	{
		float2x2 ret;

		// calculate m
		int imaxx = intGauss->getMaxX() + 1;
		int maxx = diffGauss->getMaxX() + 1 + imaxx;
		Bitmap<float> subpic(src, pixel.x - maxx, pixel.y - maxx, 2*maxx + 1, 2*maxx + 1);
						
		Bitmap<float>* diffeGauss_pic = diffGauss->Run(subpic);
		Bitmap<float3>* DIFF_O_diffGaussed_pic = new Bitmap<float3>(2*imaxx + 1, 2*imaxx + 1);
		for(int i = -imaxx; i<=imaxx; ++i)
		{
			for(int j = -imaxx; j<=imaxx; ++j)
			{
				float3& pixel = DIFF_O_diffGaussed_pic->getPixel(imaxx + i, imaxx + j);
				float dX = (diffeGauss_pic->getPixel(maxx + i + 1,	maxx + j) - diffeGauss_pic->getPixel(maxx + i - 1,	maxx + j)) / 2.0 / 255.0;
				float dY = (diffeGauss_pic->getPixel(maxx + i,		maxx + j + 1) - diffeGauss_pic->getPixel(maxx + i,	maxx + j - 1)) / 2.0 / 255.0;

				pixel.x = dX*dX;
				pixel.y = dX*dY;
				pixel.z = dY*dY;
			}
		}
		
		Bitmap<float3>* intGaussed_O_DIFF_O_diffGaussed_pic = intGauss->Run(*DIFF_O_diffGaussed_pic);

		float max = -1.0f;
		float sqrDiffSigma = od*od;
		float qvadDiffSigma = sqrDiffSigma*sqrDiffSigma;
		for(int i=-1; i<=1; ++i)
		{
			for(int j=-1; j<=1; ++j)
			{
				float3 pix = intGaussed_O_DIFF_O_diffGaussed_pic->getPixel(imaxx + i, imaxx + j);
				float tr = pix.x + pix.z;
				float det = pix.x*pix.z - pix.y*pix.y;
				float me = /*abs*/(det)*qvadDiffSigma - lambda*tr*tr*sqrDiffSigma;
				//_ASSERT(me >0);
				if(me > max)
				{
					max = me;
					x = i;
					y = j;
					ret._m11 = pix.x*sqrDiffSigma;
					ret._m12 = pix.y*sqrDiffSigma;
					ret._m21 = pix.y*sqrDiffSigma;
					ret._m22 = pix.z*sqrDiffSigma;
				}
			}
		}	
		delete diffeGauss_pic;
		delete DIFF_O_diffGaussed_pic;
		delete intGaussed_O_DIFF_O_diffGaussed_pic;
		return ret;
	}

	void Harris(Bitmap<float>& src, float diffSigma, float intSigma, float lambda, bool use_log) {
		// gauss
		Filter<float> gaussFilter = Filter<float>::createGauss(diffSigma);
		Bitmap<float>* gaussImage = gaussFilter.Run(src);

		// derivatives -> second order moment
		Bitmap<float3>* secondOrderMomentMtx = new Bitmap<float3>(gaussImage->getWidth(), gaussImage->getHeight());
		int max_width = (int)gaussImage->getWidth()-1;
		int max_height = (int)gaussImage->getHeight()-1;
		for(int i=0; i<=max_width; ++i)
		{
			for(int j=0; j<=max_height; ++j)
			{
				// clamp
				int top = std::max((int)j-1, 0);
				int bottom = std::min(max_height, (int)j+1);
				int left = std::max((int)i-1, 0);
				int right = std::min(max_width, (int)i+1);
				
				double dX = (gaussImage->getPixel(right, j) - gaussImage->getPixel(left, j)) /2.0 / 255.0;
				double dY = (gaussImage->getPixel(i, bottom) - gaussImage->getPixel(i, top)) /2.0 / 255.0;
				
				/*_ASSERT(dX <= 255.0f);
				_ASSERT(dY <= 255.0f);
				_ASSERT(dX >= -255.0f);
				_ASSERT(dY >= -255.0f);*/

				float3&  pixel = secondOrderMomentMtx->getPixel(i, j);
				pixel.x = (float)(dX*dX);
				pixel.y = (float)(dX*dY);
				pixel.z = (float)(dY*dY);
			}
		}
		
		// gauss
		Filter<float3> gaussFilter2 = Filter<float3>::createGauss(intSigma);
		Bitmap<float3>* gauss_O_secondOrderMomentMtx = gaussFilter2.Run(*secondOrderMomentMtx);
		
		float sqrDiffSigma = diffSigma*diffSigma;
		float qvadDiffSigma = diffSigma*diffSigma;
		Bitmap<float>* R_img = gaussImage;
		gaussImage = nullptr;
		for(int i=0; i<=max_width; ++i)
		{
			for(int j=0; j<=max_height; ++j)
			{
				const float3& pixel = gauss_O_secondOrderMomentMtx->getPixel(i, j);
				float det = pixel.x*pixel.z - pixel.y*pixel.y;
				float trace = pixel.x + pixel.z;
				float& dest_pixel = R_img->getPixel(i, j);
				dest_pixel = /*abs*/(det)*qvadDiffSigma - lambda*trace*trace*sqrDiffSigma;
				//dest_pixel = dest_pixel/2 +127;
			}
		}
		
		std::vector<PointScale> PointsToWork;
//		Bitmap<float>* feature_img = new Bitmap<float>(R_img->getWidth(), R_img->getHeight());
		for(int i=0; i<=max_width; ++i)
		{
			for(int j=0; j<=max_height; ++j)
			{
				bool biggest = true;
				float act_pixel = R_img->getPixel(i,j);
				for(int k=-1; biggest && k<=1; ++k)
				{
					for(int l=-1; biggest && l<=1; ++l)
					{
						if((k!=0 || l!= 0) && i+k <= max_width && i+k > 0 && j+l > 0 && j+l <= max_height)
						{
							biggest = act_pixel > R_img->getPixel(i+k, j+l);
						}
					}
				}
				float treshold = 0.000000001f;
				if(biggest && act_pixel > treshold)
				{
					PointsToWork.push_back(PointScale(Index(i, j), intSigma));
				}
			}
		}

		if(use_log)
		{
			static const float t[] = {0.7f, 0.8f, 0.9f, 1.0f, 1.1f, 1.2f, 1.3f, 1.4f};
			static const float s[] = {0.5f, 0.55f, 0.6f, 0.65f, 0.7f, 0.75f};
			std::map<float, Filter<float>* > gaussFilters;
			std::map<float, Filter<float3>* > gaussFilters3;

			for(size_t i=0; i<PointsToWork.size(); ++i)
			{
				Index& index = PointsToWork[i].first;
				
				float2x2 U;
				float _sD = diffSigma;
				float _sI = intSigma;
				float2 normFrame = float2((float)index.X, (float)index.Y);
				int2 frame = int2(index.X, index.Y);
				
				bool terminate = false;
				int z = 0;
				while(!terminate)
				{
					//normFrame = U*float2((float)frame.x, (float)frame.y);

					// search for better integrate scale
					int max_t_index = 3;
					float max_measure = -1.0f;
					float max_sI = _sI;

					// calculate maximum measure
					for(int j=0; j<sizeof(t)/sizeof(t[0]); ++j)
					{
						// get the filter
						float intsig = _sI*t[j];
						std::map<float, Filter<float>* >::const_iterator it = gaussFilters.find(intsig);
						Filter<float>* actualGauss;
						if(it == gaussFilters.cend())
						{
							actualGauss = new Filter<float>(std::move(Filter<float>::createGauss(intsig)));
							gaussFilters[intsig] = actualGauss;
						}
						else
						{
							actualGauss = it->second;
						}

						// calculate actual measure
						float res = intsig*intsig*LoG(src, actualGauss, frame.x, frame.y, max_width, max_height);

						_ASSERT(res >= 0.0f);
						if(res > max_measure)
						{
							max_measure = res;
							max_t_index = j;
							max_sI = intsig;
						}
					}
					_sI = max_sI;
					
					// search for better diff scale
				/*	int max_s_index = -1;
					float max_measure2 = -1.0f;
					float max_sD;;

					// calculate maximum measure
					for(int j=0; j<sizeof(s)/sizeof(s[0]); ++j)
					{
						// get the filter
						float difsig = _sD*s[j];
						std::map<float, Filter<float>* >::const_iterator it = gaussFilters.find(difsig);
						Filter<float>* diffGauss;
						if(it == gaussFilters.cend())
						{
							diffGauss = new Filter<float>(std::move(Filter<float>::createGauss(difsig)));
							gaussFilters[difsig] = diffGauss;
						}
						else
						{
							diffGauss = it->second;
						}

						// get the filter
						std::map<float, Filter<float3>* >::const_iterator it3 = gaussFilters3.find(_sI);
						Filter<float3>* intGauss;
						if(it3 == gaussFilters3.cend())
						{
							intGauss = new Filter<float3>(std::move(Filter<float3>::createGauss(_sI)));
							gaussFilters3[_sI] = intGauss;
						}
						else
						{
							intGauss = it3->second;
						}

						// calculate m
						int imaxx = intGauss->getMaxX();
						int maxx = diffGauss->getMaxX() + 1 + imaxx;
						Bitmap<float> subpic(src, frame.x - maxx, frame.y - maxx, 2*maxx + 1, 2*maxx + 1);
						
						Bitmap<float>* diffeGauss_pic = diffGauss->Run(subpic);
						Bitmap<float3>* DIFF_O_diffGaussed_pic = new Bitmap<float3>(2*imaxx + 1, 2*imaxx + 1);
						for(int i = -imaxx; i<=imaxx; ++i)
						{
							for(int j = -imaxx; j<=imaxx; ++j)
							{
								float3& pixel = DIFF_O_diffGaussed_pic->getPixel(imaxx + i, imaxx + j);
								float dX = (diffeGauss_pic->getPixel(maxx + i + 1,	maxx + j) - diffeGauss_pic->getPixel(maxx + i - 1,	maxx + j)) / 2.0 / 255.0;
								float dY = (diffeGauss_pic->getPixel(maxx + i,		maxx + j + 1) - diffeGauss_pic->getPixel(maxx + i,	maxx + j - 1)) / 2.0 / 255.0;
								pixel.x = dX*dX;
								pixel.y = dX*dY;
								pixel.z = dY*dY;
							}
						}
						
						Bitmap<float3>* intGaussed_O_DIFF_O_diffGaussed_pic = intGauss->Run(*DIFF_O_diffGaussed_pic);

						//calculat labda1,2
						float3 pix = intGaussed_O_DIFF_O_diffGaussed_pic->getPixel(imaxx, imaxx)	*difsig*difsig;
						float tr = pix.x + pix.z;
						float det = pix.x*pix.z - pix.y*pix.y;
						
						// calculate actual measure
						float _l_ = sqrt(tr*tr - 4*det);
						float l1 = (tr + _l_)/2.0f;
						float l2 = (tr - _l_)/2.0f;
						float me = l2/l1;

						delete diffeGauss_pic;
						delete DIFF_O_diffGaussed_pic;
						delete intGaussed_O_DIFF_O_diffGaussed_pic;
						
						_ASSERT(l1 > 0);
						_ASSERT(l2 > 0);
						_ASSERT(me > 0);
						if(me > max_measure2)
						{
							max_measure2 = me;
							max_s_index = j;
							max_sD = difsig;
						}
					}
					_sD = max_sD;*/

					// get the filter
					std::map<float, Filter<float>* >::const_iterator it = gaussFilters.find(_sD);
					Filter<float>* diffGauss;
					_ASSERT(it != gaussFilters.cend());
					diffGauss = it->second;

						// get the filter
						std::map<float, Filter<float3>* >::const_iterator it3 = gaussFilters3.find(_sI);
						Filter<float3>* intGauss;
						if(it3 == gaussFilters3.cend())
						{
							intGauss = new Filter<float3>(std::move(Filter<float3>::createGauss(_sI)));
							gaussFilters3[_sI] = intGauss;
						}
						else
						{
							intGauss = it3->second;
						}

					int off_x, off_y;
					float2x2 mtx = calcMeasureMex(off_x, off_y, src, diffGauss, intGauss, frame, lambda, _sD);

					/*float _d = mtx._m11*mtx._m22 - mtx._m12*mtx._m21;
					float _tr = mtx._m11+mtx._m22;
					float _s = sqrt(_d);
					float _t = sqrt(_tr + 2*_s);

					float2x2 sqrtmtx;
					if(abs(_t) > 0.0001)
					{
						sqrtmtx._m11 = mtx._m11/_t;
						sqrtmtx._m12 = mtx._m12/_t;
						sqrtmtx._m21 = mtx._m21/_t;
						sqrtmtx._m22 = mtx._m22/_t;
					}
					else
					{
						float s =  mtx._m11 +  mtx._m22 +  2*_s;
						sqrtmtx._m11 = mtx._m11*s;
						sqrtmtx._m12 = mtx._m12*s;
						sqrtmtx._m21 = mtx._m21*s;
						sqrtmtx._m22 = mtx._m22*s;
					}

					normFrame = normFrame + U*float2(off_x, off_y);

					U = sqrtmtx * U;*/

					// seacrh for better neighbour
					/*int max_off_x = 0;
					int max_off_y = 0;
					for(int j=-1; j<=1; ++j)
					{
						for(int k=-1; k<=1; ++k)
						{
							if(j==0 && k==0) 
								continue;

							int x = actual.first.X + j;
							int y = actual.first.Y + k;

							if(x<0 || y<0 || x>max_width || y>max_height)
								continue;

							float res = max_scale*max_scale*LoG(src, actualGauss, x, y, max_width, max_height);
							if(res > max_measure)
							{
								max_measure = res;
								max_off_x = j;
								max_off_y = k;
							}
						}
					}*/

					terminate = (off_x == 0 && off_y == 0 && max_t_index == 3) || z++ >15;
					index.X += off_x;
					index.Y += off_y;
					frame.x = index.X;
					frame.y = index.Y;
				}
				PointsToWork[i].second = _sI;
			}

			// todo set mert duplicated

			for(const std::pair<float, Filter<float>* >& pair : gaussFilters)
			{
				delete pair.second;
			}
			gaussFilters.clear();
		}
			
		for(size_t i=0; i<PointsToWork.size(); ++i)
		{
			PointScale& act = PointsToWork[i];
			src.addFeature(act.first.X, act.first.Y, act.second);
			src.addKeyPoint(act.first.X, act.first.Y, act.second);
		}
#if 0
		delete secondOrderMomentMtx;
		delete gauss_O_secondOrderMomentMtx;
		return gaussImage;
#else
		delete secondOrderMomentMtx;
		delete gauss_O_secondOrderMomentMtx;
		delete R_img;
		delete gaussImage;
#endif
	}

	Bitmap<float>* Hesse(Bitmap<float>& src, float diffSigma) {
		// gauss
		Filter<float> gaussFilter = Filter<float>::createGauss(diffSigma);
		Bitmap<float>* gaussImage = gaussFilter.Run(src);

		// derivated
		Bitmap<float>* det_image = new Bitmap<float>(gaussImage->getWidth(), gaussImage->getHeight());
		int max_width = (int)gaussImage->getWidth()-1;
		int max_height = (int)gaussImage->getHeight()-1;
		for(int i=0; i<=max_width; ++i)
		{
			for(int j=0; j<=max_height; ++j)
			{
				// clamp
				int top = std::max((int)j-1, 0);
				int bottom = std::min(max_height, (int)j+1);
				int left = std::max((int)i-1, 0);
				int right = std::min(max_width, (int)i+1);
				
				// pixels
				float p_center = gaussImage->getPixel(i, j) / 255.0f;
				float p_top = gaussImage->getPixel(i, top) / 255.0f;
				float p_bottom = gaussImage->getPixel(i, bottom) / 255.0f;
				float p_left = gaussImage->getPixel(left, j) / 255.0f;
				float p_right = gaussImage->getPixel(right, j) / 255.0f;

				// 1 - 1
				double dX = (p_right - p_left) / 2.0;
				double dY = (p_bottom - p_top) / 2.0;
				
				// 1 -2 1 <- faust
				double dXX = (p_right - 2.0*p_center + p_left) /2.0;
				double dYY = (p_bottom - 2.0*p_center + p_top) /2.0;

				//  1			   1 -2  1
				// -2 * 1 -2  1 = -2  4 -2
				//  1			   1 -2  1
				double dXY = (gaussImage->getPixel(left, top) + gaussImage->getPixel(left, bottom)
					+ gaussImage->getPixel(right, top) + gaussImage->getPixel(right, bottom)) / 255.0f
					- 2.0 * (p_top + p_bottom + p_left + p_right - 2.0 * p_center);
				
				/*_ASSERT(dX <= 255.0f);
				_ASSERT(dY <= 255.0f);
				_ASSERT(dX >= -255.0f);
				_ASSERT(dY >= -255.0f);*/
				
				det_image->getPixel(i,j) = 0;
				double det = dXX*dYY - dXY*dXY;
				if(det >= 0.0f)
				{
					double tr = dXX + dYY;
					double s = (tr*tr)/det;
					if (s <= 10.0)
					{
						det_image->getPixel(i,j) = (float)det;
					}
				}
			}
		}
		
		for(int i=0; i<=max_width; ++i)
		{
			for(int j=0; j<=max_height; ++j)
			{
				// clamp
				int top = std::max(std::min(max_height, (int)j-1), 0);
				int bottom = std::max(std::min(max_height, (int)j+1), 0);
				int left = std::max(std::min(max_width, (int)i-1), 0);
				int right = std::max(std::min(max_width, (int)i+1), 0);
				
				float p_center = det_image->getPixel(i, j);

				if(p_center > 0.000001f)
				{
					bool biggest = true;
					for(int k=-1; biggest && k<=1; ++k)
					{
						for(int l=-1; biggest && l<=1; ++l)
						{
							if(l == 0 && k == 0)
								continue;

							int x = std::max(std::min(max_width, i+k), 0);
							int y = std::max(std::min(max_height, j+l), 0);

							biggest = (p_center > det_image->getPixel(x, y));
						}
					}
				
					if(biggest)
					{
						src.addKeyPoint(i,j, diffSigma);
						src.addFeature(i,j, 1.0f);
					}
				}
			}
		}

		delete gaussImage;
		delete det_image;
		return nullptr;
	}
}